package com.feishi.core.util.common;

/**
 * Created with IntelliJ IDEA.
 * Description:
 * User: caixq
 * Date: 2020-04-03
 * Time: 下午2:07
 */
public class PID {
    double SetSpeed;//定义设定值
    double ActualSpeed;//定义实际值
    double err;//定义偏差值
    double err_last;//定义上一个偏差值
    double Kp,Ki,Kd;//定义比例、积分、微分系数
    double voltage;//定义电压值(控制执行器的变量)
    double integral;//定义积分值

    public static void main(String[] args) {
        double base=0;
        double target=10;
        PID pid=new PID();
        pid.SetSpeed=0.0;
        pid.ActualSpeed=0.0;
        pid.err=0.0;
        pid.err_last=0.0;
        pid.voltage=0.0;
        pid.integral=0.0;
        pid.Kp=0.2;
        pid.Ki=0.015;
        pid.Kd=0.2;
        new Thread(()->{
            try {
                Thread.sleep(10);
            } catch (InterruptedException e) {
                e.printStackTrace();
            }

        }).start();
    }
   public double PID_realize(double speed){
        PID pid=this;
        pid.SetSpeed=speed;
        pid.err=pid.SetSpeed-pid.ActualSpeed;
        pid.integral+=pid.err;
        pid.voltage=pid.Kp*pid.err+pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);
        pid.err_last=pid.err;
        pid.ActualSpeed=pid.voltage*1.0;
        return pid.ActualSpeed;
    }
}
class Pid1{
    double SetSpeed;//定义设定值
    double ActualSpeed;//定义实际值
    double err;//定义偏差值
    double err_next;//定义下一个偏差值
    double err_last;//定义上一个偏差值
    double Kp,Ki,Kd;//定义比例、积分、微分系数
    double voltage;//定义电压值(控制执行器的变量)
    double integral;//定义积分值
    public void PID_init(){
        Pid1 pid=this;
        pid.SetSpeed=0.0;
        pid.ActualSpeed=0.0;
        pid.err=0.0;
        pid.err_next=0.0;
        pid.err_last=0.0;
        pid.voltage=0.0;
        pid.integral=0.0;
        pid.Kp=0.2;
        pid.Ki=0.015;
        pid.Kd=0.2;
    }
    public double PID_realize(float speed){
        Pid1 pid=this;
        pid.SetSpeed=speed;
        pid.err=pid.SetSpeed-pid.ActualSpeed;
        double
                incrementSpeed=pid.Kp*(pid.err-pid.err_next)+pid.Ki*pid.err+pid.Kd*(pid.err-2*pid.err_next+pid.err_last);
        pid.ActualSpeed+=incrementSpeed;
        pid.err_last=pid.err_next;
        pid.err_next=pid.err;
        return pid.ActualSpeed;
    }
}
